/**************************************************************************
File:        positiondsp.cpp
Description:

Copyright:   (C) 2001 by ZJ Laczik
Email:       Zsolt.Laczik@eng.ox.ac.uk
****************************************************************************
Change log:
   begin      Fri Mar 16 2001
   15/2/2012	QT4 update
***************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include <QPainter>
#include <QMouseEvent>

#include "positiondsp.h"

#define PTR_REG_SIZE 10
#define STP_SIZE_L 1
#define STP_SIZE_M 2
#define STP_SIZE_R 5
#define STP_MLT_CTRL 10
#define STP_MLT_SHIFT 100
#define STP_MLT_CTRLSHIFT 1000
#define NO_AXIS -1
#define ZERO_AXIS 0
#define X_AXIS 1
#define Y_AXIS 2
#define Z_AXIS 3

PositionDsp::PositionDsp(QWidget *parent ) : QWidget(parent) {
    float min,max,pos;

    min = -50.0e3 ;
    max =  50.0e3 ;
    mMinX = min;
    mMaxX = max;
    mMinY = min;
    mMaxY = max;
    min = -5.0e3 ;
    max =  5.0e3 ;
    mMinZ = min;
    mMaxZ = max;
    pos = 0.0;
    mXPos = pos;
    mYPos = pos;
    mZPos = pos;

    mMultiplier = 1.0;

    mW = width();
    mH = height();

    mZoom = 1.0;

    Set3DRotation(0.65*M_PI, 0.3*M_PI, 0.0);
    CalcMaxRange();
    CalcEndPoints();

    setCursor( Qt::SizeAllCursor );
    setMouseTracking(true);

    repaint();
}

PositionDsp::~PositionDsp() {
}

void PositionDsp::SetXPos(float pos) {
    mXPos = pos;
    repaint();
}

void PositionDsp::SetYPos(float pos) {
    mYPos = pos;
    repaint();
}

void PositionDsp::SetZPos(float pos) {
    mZPos = pos;
    repaint();
}

void PositionDsp::SetPos(float x, float y, float z) {
    mXPos = x;
    mYPos = y;
    mZPos = z;
    repaint();
}

void PositionDsp::SetXLimits(float min, float max) {
    mMinX = min;
    mMaxX = max;
    repaint();
}

void PositionDsp::SetYLimits(float min, float max) {
    mMinY = min;
    mMaxY = max;
    repaint();
}

void PositionDsp::SetZLimits(float min, float max) {
    mMinZ = min;
    mMaxZ = max;
    repaint();
}

void PositionDsp::paintEvent(QPaintEvent* event) {
    mUpdateRect = event->rect();
    Redraw();
}

void PositionDsp::resizeEvent( QResizeEvent *e ) {
    mW = width();
    mH = height();
    CalcEndPoints();
}

void PositionDsp::keyPressEvent( QKeyEvent* e )
{
    // did we get here because of auto-repeat?
    if (e->isAutoRepeat())
        return;
    // set multiplier if the key was 1, 2 or 3
    switch ( e->key() ) {
    case Qt::Key_1:
        mMultiplier = 0.1;
        break;
    case Qt::Key_2:
        mMultiplier = 0.01
        ;
        break;
    case Qt::Key_3:
        mMultiplier = 0.001;
        break;
    default:
        e->ignore();
        break;
    }
}

void PositionDsp::keyReleaseEvent( QKeyEvent* e )
{
    // did we get here because of auto-repeat?
    if (e->isAutoRepeat())
        return;
    // clear multiplier flag if the key was 1, 2 or 3
    switch ( e->key() ) {
    case Qt::Key_1:
        mMultiplier = 1.0;
        break;
    case Qt::Key_2:
        mMultiplier = 1.0;
        break;
    case Qt::Key_3:
        mMultiplier = 1.0;
        break;
    default:
        e->ignore();
        break;
    }
}

void PositionDsp::enterEvent( QEvent *e ) {
    // grab the focus as mouse enters dsp.
    setFocus();
}

void PositionDsp::leaveEvent( QEvent *e ) {
    // this will not stop key events... - but it does not matter
    clearFocus();
}

void PositionDsp::mousePressEvent( QMouseEvent *e ) {
    // not using this, movement is triggered by mouse button release
}

void PositionDsp::mouseReleaseEvent( QMouseEvent *e ) {
    float distance=0.0;

    // are we looking to move?
    if (!mEnableMoves || mAxis==NO_AXIS)
        return;
    // yes, check how much
    mEnableMoves = false;
    switch (e->button()) {
    case Qt::LeftButton:
        distance = 1.0e3;
        break;
    case Qt::MidButton:
        distance = 2.0e3;
        break;
    case Qt::RightButton:
        distance = 5.0e3;
        break;
    }
    
    distance *= mMultiplier;

    if (mAxis==Z_AXIS)
      distance /= 10.0;
    
    distance *= mDir;

    // send the move signals
    if (mAxis==ZERO_AXIS)
        emit( MoveTo(0.0,0.0,0.0) );
    else
        emit( Move(mAxis,distance) );

    mEnableMoves = true;
}

void PositionDsp::mouseDoubleClickEvent( QMouseEvent *e ) {
}

void PositionDsp::mouseMoveEvent( QMouseEvent *e ) {
    // change mouse curser shape when in right region
    if (!mEnableMoves) {
        mAxis = NO_AXIS;
        setCursor( Qt::CrossCursor );
    }
    else if ( (e->pos()-mZZ).manhattanLength()<PTR_REG_SIZE ) {
        mAxis = ZERO_AXIS;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mXP).manhattanLength()<PTR_REG_SIZE )
    {
        mAxis = X_AXIS;
        mDir = 1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mXM).manhattanLength()<PTR_REG_SIZE )
    {
        mAxis = X_AXIS;
        mDir = -1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mYP).manhattanLength()<PTR_REG_SIZE )
    {
        mAxis = Y_AXIS;
        mDir = 1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mYM).manhattanLength()<PTR_REG_SIZE )
    {
        mAxis = Y_AXIS;
        mDir = -1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mZP).manhattanLength()<PTR_REG_SIZE ) {
        mAxis = Z_AXIS;
        mDir = 1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else if ( (e->pos()-mZM).manhattanLength()<PTR_REG_SIZE ) {
        mAxis = Z_AXIS;
        mDir = -1.0;
        setCursor( Qt::SizeAllCursor );
    }
    else {
        mAxis = NO_AXIS;
        setCursor( Qt::CrossCursor );
    }
}

void PositionDsp::Redraw() {
    float ftmp;
    float x1,y1,z1;
    int sx1,sy1;
    QColor c;

    QPainter p(this);

    p.setClipRect(mUpdateRect);
    mW = width();
    mH = height();

    // paint the background
    c.setRgb( 64, 64, 64 );
    p.setPen(c);
    p.setBrush(c);
    p.drawRect( 0, 0, mW, mH );

    // draw the axes
    c.setRgb( 192, 192, 192 );
    p.setPen(c);
    float aw = 0.01*(mMax-mMin);
    float al = 0.05*(mMax-mMin);
    Draw3DLine(p, mMin, 0.0, 0.0,   mMax, 0.0, 0.0);
    Draw3DLine(p, mMax-al, -aw, 0.0,   mMax, 0.0, 0.0);
    Draw3DLine(p, mMax-al,  aw, 0.0,   mMax, 0.0, 0.0);
    Draw3DLine(p, mMax-al, 0.0, -aw,   mMax, 0.0, 0.0);
    Draw3DLine(p, mMax-al, 0.0,  aw,   mMax, 0.0, 0.0);

    Draw3DLine(p, 0.0, mMin, 0.0,   0.0, mMax, 0.0);
    Draw3DLine(p, -aw,  mMax-al, 0.0,   0.0, mMax, 0.0);
    Draw3DLine(p,  aw,  mMax-al, 0.0,   0.0, mMax, 0.0);
    Draw3DLine(p, 0.0,  mMax-al, -aw,   0.0, mMax, 0.0);
    Draw3DLine(p, 0.0,  mMax-al,  aw,   0.0, mMax, 0.0);

    Draw3DLine(p, 0.0, 0.0, mMin,   0.0, 0.0, mMax);
    Draw3DLine(p, 0.0, -aw, mMax-al,   0.0, 0.0, mMax);
    Draw3DLine(p, 0.0,  aw, mMax-al,   0.0, 0.0, mMax);
    Draw3DLine(p, -aw, 0.0, mMax-al,   0.0, 0.0, mMax);
    Draw3DLine(p,  aw, 0.0, mMax-al,   0.0, 0.0, mMax);

    // draw scale markers
    ftmp = 5.0*pow10(int(log10(mMax)));
    Rotate3D( ftmp, 0.0, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-1,sy1-1,2,2);
    Rotate3D( 0.0, ftmp, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-1,sy1-1,2,2);
    Rotate3D( 0.0, 0.0, ftmp, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-1,sy1-1,2,2);

    p.setFont(QFont("Arial", 8));
    c.setRgb( 255, 255, 255 );
    p.setPen(c);
    
    Rotate3D(ftmp, 0.0, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawText( QPoint(sx1,sy1+4), QString("   %1") .arg(ftmp/mZoom) );
    Rotate3D(0.0, ftmp, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawText( QPoint(sx1,sy1+4), QString("   %1") .arg(ftmp/mZoom) );
    Rotate3D(0.0, 0.0, ftmp, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawText( QPoint(sx1,sy1+4), QString("   %1") .arg(ftmp/10.0/mZoom) );
        
    // draw markers on the axes
    c.setRgb( 0, 255, 0 );
    p.setPen(c);
    p.setBrush(c);

    Rotate3D(mZoom*mXPos, 0.0, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-2,sy1-2,4,4);

    Rotate3D(0.0, mZoom*mYPos, 0.0, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-2,sy1-2,4,4);

    Rotate3D(0.0, 0.0, 10.0*mZoom*mZPos, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-2,sy1-2,4,4);

    Draw3DLine(p, mZoom*mXPos, 0.0, 0.0,   mZoom*mXPos, mZoom*mYPos, 0.0);
    Draw3DLine(p, mZoom*mXPos, mZoom*mYPos, 0.0,  0.0, mZoom*mYPos, 0.0);
    Draw3DLine(p, mZoom*mXPos, mZoom*mYPos, 0.0,  mZoom*mXPos, mZoom*mYPos, mZoom*10.0*mZPos);
    
    // draw marker for object
    c.setRgb( 255, 0, 0 );
    p.setPen(c);
    p.setBrush(c);
    Rotate3D(mZoom*mXPos, mZoom*mYPos, 10.0*mZoom*mZPos, x1, y1, z1);
    World2Scr(x1,y1,sx1,sy1);
    p.drawEllipse(sx1-5,sy1-5,10,10);
}

void PositionDsp::Set3DRotation(float a, float b, float c) {
    mRM[0][0] = (cos(a)*cos(c)-cos(b)*sin(a)*sin(c));
    mRM[0][1] = (cos(c)*sin(a)+cos(a)*cos(b)*sin(c));
    mRM[0][2] = (sin(b)*sin(c));
    mRM[1][0] = (-cos(b)*cos(c)*sin(a)-cos(a)*sin(c));
    mRM[1][1] = (cos(a)*cos(b)*cos(c)-sin(a)*sin(c));
    mRM[1][2] = (cos(c)*sin(b));
    mRM[2][0] = (sin(a)*sin(b));
    mRM[2][1] = (-cos(a)*sin(b));
    mRM[2][2] = (cos(b));
}

void PositionDsp::CalcMaxRange() {
    float rx,ry,rz;
    rx = fabs(mMaxX-mMinX);
    ry = fabs(mMaxY-mMinY);
    rz = fabs(mMaxZ-mMinZ);
    mMin = (mMinX<mMinY) ? mMinX : mMinY ;
    mMin = (mMin<mMinZ)  ? mMin :  mMinZ ;
    mMax = (mMaxX>mMaxY) ? mMaxX : mMaxY ;
    mMax = (mMax>mMaxZ)  ? mMax :  mMaxZ ;
    mMin *= sqrt(2);
    mMax *= sqrt(2);
}

void PositionDsp::World2Scr(float x, float y, int &nx, int &ny) {
    nx = (int)(mW*(x-mMin)/(mMax-mMin));
    ny = (int)(mH*(1.0-(y-mMin)/(mMax-mMin)));
}

void PositionDsp::Rotate3D(float x, float y, float z, float &nx, float &ny, float &nz) {
    nx = mRM[0][0]*x + mRM[0][1]*y + mRM[0][2]*z;
    ny = mRM[1][0]*x + mRM[1][1]*y + mRM[1][2]*z;
    nz = mRM[2][0]*x + mRM[2][1]*y + mRM[2][2]*z;
}

void PositionDsp::Draw3DLine(QPainter &p, float xx1, float yy1, float zz1, float xx2, float yy2, float zz2) {
    float x1,x2,y1,y2,z1,z2;
    int sx1,sx2,sy1,sy2;

    Rotate3D(xx1, yy1, zz1, x1, y1, z1);
    Rotate3D(xx2, yy2, zz2, x2, y2, z2);
    World2Scr(x1,y1,sx1,sy1);
    World2Scr(x2,y2,sx2,sy2);
    p.drawLine(sx1,sy1,sx2,sy2);
}

void PositionDsp::CalcEndPoints() {
    float x,y,z;
    int sx,sy;

    Rotate3D(0.0, 0.0, 0.0, x, y, z);
    World2Scr(x,y,sx,sy);
    mZZ = QPoint(sx,sy);

    Rotate3D(mMin, 0.0, 0.0, x, y, z);
    World2Scr(x,y,sx,sy);
    mXM = QPoint(sx,sy);
    Rotate3D(0.0, mMin, 0.0, x, y, z);
    World2Scr(x,y,sx,sy);
    mYM = QPoint(sx,sy);
    Rotate3D(0.0, 0.0, mMin, x, y, z);
    World2Scr(x,y,sx,sy);
    mZM = QPoint(sx,sy);

    Rotate3D(mMax, 0.0, 0.0, x, y, z);
    World2Scr(x,y,sx,sy);
    mXP = QPoint(sx,sy);
    Rotate3D(0.0, mMax, 0.0, x, y, z);
    World2Scr(x,y,sx,sy);
    mYP = QPoint(sx,sy);
    Rotate3D(0.0, 0.0, mMax, x, y, z);
    World2Scr(x,y,sx,sy);
    mZP = QPoint(sx,sy);
}

void PositionDsp::EnableMoves(bool state) {
    mEnableMoves = state;
}
